#pragma once
#include <deque>
#include <Eigen/Dense>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/shared_ptr.hpp>  // boost智能指针

// -------------------------- 引入项目已有的头文件（修正路径） --------------------------
// 1. 正确引入CloudData（项目实际路径：sensor_data/cloud_data.hpp）
#include "lio_ndt/sensor_data/cloud_data.hpp"
// 2. 若项目有Frame定义（如sensor_data/frame.hpp），引入；若无，直接定义（避免重复）
// #include "lio_ndt/sensor_data/frame.hpp"  // 若存在，解开注释；若不存在，保留下方Frame定义

// 3. 配准相关头文件（路径不变）
#include "lio_ndt/method/registration.h"
#include "lio_ndt/method/optimized_ICP_GN.h"

// -------------------------- 若项目无现成Frame定义，在此定义（避免重复） --------------------------
// 注：若项目中已有 sensor_data/frame.hpp，删除下方Frame定义，改为引入头文件
namespace lio_ndt {
struct Frame {
    CloudData cloud_data;          // 帧对应的点云数据（使用项目已有的CloudData）
    Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();  // 帧的全局位姿（4x4变换矩阵）
};
}  // namespace lio_ndt

namespace lio_ndt {

class FrontEnd {
public:
    // 类型别名：复用CloudData中的嵌套类型，避免重复定义
    using PointT = CloudData::POINT;
    using CloudPtr = CloudData::CLOUD_PTR;

    // 构造函数/析构函数
    FrontEnd();
    ~FrontEnd() = default;

    // -------------------------- 核心接口（参数类型与项目一致） --------------------------
    // 输入：项目已有的CloudData类型；输出：当前帧全局位姿
    Eigen::Matrix4f Update(const CloudData& cloud_data);
    // 设置初始位姿
    bool SetInitPose(const Eigen::Matrix4f& init_pose);
    // 地图获取（输出参数为CloudData::CLOUD_PTR，与项目类型一致）
    bool GetNewLocalMap(CloudPtr& local_map_ptr);
    bool GetNewGlobalMap(CloudPtr& global_map_ptr);
    bool GetCurrentScan(CloudPtr& current_scan_ptr);

private:
    // 关键帧更新：维护局部/全局地图
    void UpdateNewFrame(const Frame& new_key_frame);

    // -------------------------- 成员变量（类型与项目一致） --------------------------
    // 配准器指针（boost智能指针，匹配PCL和项目类型）
    boost::shared_ptr<Registration> matcher_;
    // 优化版ICP指针（若后续报boost/std冲突，可改为boost::unique_ptr）
    std::unique_ptr<OptimizedICPGN> opti_icp_;

    // 地图相关（使用CloudData::CLOUD_PTR，与项目类型统一）
    CloudPtr local_map_ptr_;    // 局部地图点云
    CloudPtr global_map_ptr_;   // 全局地图点云
    CloudPtr result_cloud_ptr_; // 配准后结果点云

    // 关键帧队列（存储Frame类型，与项目数据结构一致）
    std::deque<Frame> local_map_frames_;  // 局部地图关键帧
    std::deque<Frame> global_map_frames_; // 全局地图关键帧
    Frame current_frame_;                 // 当前处理帧

    // 体素滤波器（点类型为CloudData::POINT，与项目一致）
    pcl::VoxelGrid<PointT> cloud_filter_;    // 输入点云下采样
    pcl::VoxelGrid<PointT> local_map_filter_;// 局部地图下采样
    pcl::VoxelGrid<PointT> display_filter_;  // 可视化点云下采样

    // 其他参数
    Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity();  // 初始位姿
    bool has_new_local_map_ = false;  // 局部地图更新标记
    bool has_new_global_map_ = false; // 全局地图更新标记
};

}  // namespace lio_ndt
